#include <cassert>

template <class State, class Heuristic, class Successors>
BestFirstSearch<State, Heuristic, Successors>::BestFirstSearch( )
  : m_Allocator(), 
    m_OpenList(), 
	m_OpenHashMap(),
	m_ClosedList(),
	m_HeuristicFunction(), 
	m_SuccessorsOf(), 
	m_pNodePath(0)
{
}

template <class State, class Heuristic, class Successors>
BestFirstSearch<State, Heuristic, Successors>::~BestFirstSearch( )
{
}

//
//1. Start with OPEN containing just the initial state
//2. Until a goal state is found or there is no node left on OPEN do:
//  i.   Pick the best node on OPEN
//  ii.  Generate its successors
//  iii. For each successor do:
//      a. If it has not been generated before: evaluate it, add it to OPEN and record its parent
//      b. Otherwise: change the parent if this new path is better than previous one.

template <class State, class Heuristic, class Successors>
bool BestFirstSearch<State, Heuristic, Successors>::find( const State &start, const State &end )
{
	//cleanup( );
	assert( m_OpenList.empty( ) == true && m_ClosedList.empty( ) == true );

#ifdef _BESTFIRSTSEARCH_MEMORY_DEBUG
	nAllocations = 0;
#endif

	// put the start node in the queue...
	Node *pStartNode = m_Allocator.allocate( 1 );
#ifdef _BESTFIRSTSEARCH_MEMORY_DEBUG
	nAllocations++;
#endif
	m_Allocator.construct( pStartNode, Node( start, m_HeuristicFunction( start, end ), NULL ) );
	openListPush( pStartNode );

	while( !m_OpenList.empty( ) )
	{
		Node *pCurrentNode = openListBestCandidate( );
		m_ClosedList.insert( pair<State, Node *>(pCurrentNode->state, pCurrentNode) );
		openListPop( );


		if( pCurrentNode->state == end )
		{
			m_pNodePath = pCurrentNode;
			return true;
		}

		SuccessorCollection &successors = m_SuccessorsOf( pCurrentNode->state );
		SuccessorCollection::iterator itr;

		// for each successor...
		for( itr = successors.begin( ); itr != successors.end( ); ++itr )
		{
			assert( *itr != pCurrentNode->state );
			ClosedCollection::iterator visitedItr = m_ClosedList.find( *itr );
			


			if( visitedItr == m_ClosedList.end( ) ) 
			{
				Node *pExistingOpenNode = openListFind( *itr );

				if( pExistingOpenNode == NULL )
				{
					Node *pNewNode = m_Allocator.allocate( 1 );
					
#ifdef _BESTFIRSTSEARCH_MEMORY_DEBUG
					nAllocations++;
#endif

					unsigned int h = m_HeuristicFunction( *itr, end );

					// calculate the cost and place in the queue...
					m_Allocator.construct( pNewNode, Node( *itr, h, pCurrentNode ) );					
					openListPush( pNewNode );
				}
				else
				{
					unsigned int h = m_HeuristicFunction( pExistingOpenNode->state, end );
					if( h < pExistingOpenNode->h )
					{	
						pExistingOpenNode->h = h;
						pExistingOpenNode->parent = pCurrentNode;
						openListHeapify( );
					}
				}
			}
		}
	}

	return false;
}


template <class State, class Heuristic, class Successors>
void BestFirstSearch<State, Heuristic, Successors>::cleanup( )
{
#ifdef _BESTFIRSTSEARCH_MEMORY_DEBUG
	assert( nAllocations == m_OpenList.size( ) + m_ClosedList.size( ) );
#endif

	// free memory on the queue...
	for( OpenCollection::iterator itr = m_OpenList.begin( );
		 itr != m_OpenList.end( );
		 ++itr )
	{
		m_Allocator.destroy( *itr );
		m_Allocator.deallocate( *itr, 1 );

#ifdef _BESTFIRSTSEARCH_MEMORY_DEBUG
		nAllocations--;
#endif
	}
	m_OpenList.clear( );

	//while( !m_OpenList.empty( ) )
	//{
	//	Node *pCurrentNode = m_OpenList.top( );
	//	m_OpenList.pop( );

	//	m_Allocator.destroy( pCurrentNode );
	//	m_Allocator.deallocate( pCurrentNode, 1 );
	//	nAllocations--;
	//}

	
	for( ClosedCollection::iterator itr = m_ClosedList.begin( );
		 itr != m_ClosedList.end( );
		 ++itr )
	{
		m_Allocator.destroy( itr->second );
		m_Allocator.deallocate( itr->second, 1 );
#ifdef _BESTFIRSTSEARCH_MEMORY_DEBUG
		nAllocations--;
#endif
	}

	m_ClosedList.clear( );
	m_OpenHashMap.clear( );
	assert( m_OpenList.empty( ) == true && m_ClosedList.empty( ) == true );
	m_pNodePath = NULL; // reset to NULL...
}